#pragma once
#ifndef Geo_h__
#define Geo_h__

#include <cmath>
#include <eastl/algorithm.h>
#include "Common.h"

using eastl::max;
using eastl::min;

//http://www.gamedev.net/page/resources/_/technical/game-programming/swept-aabb-collision-detection-and-response-r3084

///************************************************************************/
/// Functions
///************************************************************************/

inline float DegToRad(const float deg) 
{
	return (deg*3.1415926f)/180.0f;
}

#define Min3(x,y,z)  ((y) <= (z) ? ((x) <= (y) ? (x) : (y)) : ((x) <= (z) ? (x) : (z)))
#define Max3(x,y,z)  ((y) >= (z) ? ((x) >= (y) ? (x) : (y)) : ((x) >= (z) ? (x) : (z)))

///************************************************************************/
/// Point
///************************************************************************/

struct Vec
{
	union	{ struct { float X; float Y; }; float A[2]; };

	Vec() : X(0), Y(0) {}
	Vec(float x, float y) : X(x), Y(y) {}

	static Vec FromAngle(float angle) { return Vec(cosf(angle),sinf(angle)); }

	Vec operator*(float f) const { return Vec(X*f, Y*f); }
	Vec& operator*=(float f) { X*=f; Y*=f; return *this; }
	Vec operator/(float f) const { return Vec(X/f, Y/f); }
	Vec operator*(const Vec& other) const { return Vec(X*other.X, Y*other.Y); }
	Vec operator/(const Vec& other) const { return Vec(X/other.X, Y/other.Y); }
	Vec operator+(const Vec& other) const { return Vec(X+other.X, Y+other.Y); }
	Vec operator-(const Vec& other) const { return Vec(X-other.X, Y-other.Y); }
	Vec operator-() const { return Vec(-X, -Y); }

	Vec& operator+=(const Vec& other) { X += other.X; Y += other.Y; return *this; }
	Vec& operator-=(const Vec& other) { X -= other.X; Y -= other.Y; return *this; }

	float DotProduct(const Vec& other) const { return X*other.X + Y*other.Y; }

	float Length() const { return sqrtf(X*X+Y*Y); }
	float LengthSquared() const { return X*X+Y*Y; }
	float Angle() const { return atan2(Y, X); }
	float Angle(const Vec& other) const { return atan2(Y-other.Y, X-other.X); }
	Vec Normalized() const { return (LengthSquared()==0)?Vec():((*this)/Length()); }
	Vec RotatedAround(const Vec& pivot, float angle) const
	{
		if (angle == 0) return *this;
		Vec res;

		float sa = sinf(angle);
		float ca = cosf(angle);
		res.X = ((X-pivot.X)*ca - (Y-pivot.Y)*sa) + pivot.X;
		res.Y = ((X-pivot.X)*sa + (Y-pivot.Y)*ca) + pivot.Y;

		return res;
	}

	Vec DirTo(const Vec& other) const { return (other-*this).Normalized(); }
	float Distance(const Vec& other) const { return (other-*this).Length(); }
	float DistanceSquared(const Vec& other) const { return (other-*this).LengthSquared(); }

	Vec Floored() const { return Vec(floor(X), floor(Y)); }
};

inline Vec operator*(float f, const Vec& p)
{
	return p*f;
}


struct Vec3
{
	union
	{
		struct{ float X; float Y; float Z;};
		float V[3];
	};

	Vec3() : X(0), Y(0), Z(0) {}
	Vec3(float x, float y, float z) : X(x), Y(y), Z(z) {}

	Vec3 operator*(float f) const { return Vec3(X*f, Y*f, Z*f); }
	Vec3& operator*=(float f) { X*=f; Y*=f; Z*=f; return *this; }
	Vec3 operator/(float f) const { float inv = 1.0f/f; return Vec3(X*inv, Y*inv, Z*inv); }
	Vec3 operator*(const Vec3& other) const { return Vec3(X*other.X, Y*other.Y, Z*other.Z); }
	Vec3 operator/(const Vec3& other) const { return Vec3(X/other.X, Y/other.Y, Z/other.Z); }
	Vec3 operator+(const Vec3& other) const { return Vec3(X+other.X, Y+other.Y, Z+other.Z); }
	Vec3 operator-(const Vec3& other) const { return Vec3(X-other.X, Y-other.Y, Z-other.Z); }
	Vec3 operator-() const { return Vec3(-X, -Y, -Z); }

	Vec3& operator+=(const Vec3& other) { X += other.X; Y += other.Y; Z += other.Z; return *this; }

	float DotProduct(const Vec3& other) const { return X*other.X + Y*other.Y + Z*other.Z; }

	float Length() const { return sqrtf(X*X+Y*Y+Z*Z); }
	float LengthSquared() const { return X*X+Y*Y+Z*Z; }
	//float Angle(const Vec3& other) const { return atan2(Y-other.Y, X-other.X); }
	Vec3 Normalized() const { return (LengthSquared()==0)?Vec3():((*this)/Length()); }
	/*
	Vec3 RotatedAround(const Vec3& pivot, float angle) const
	{
		if (angle == 0) return *this;
		Vec3 res;

		float sa = sinf(angle);
		float ca = cosf(angle);
		res.X = ((X-pivot.X)*ca - (Y-pivot.Y)*sa) + pivot.X;
		res.Y = ((X-pivot.X)*sa + (Y-pivot.Y)*ca) + pivot.Y;

		return res;
	}
	*/

};

///************************************************************************/
/// Rect
///************************************************************************/

struct Rect
{
	Vec P1, P2;

	Rect() {}
	Rect(const Vec& p1, const Vec& p2) : P1(p1), P2(p2) {}
	Rect(float x1, float y1, float x2, float y2) : P1(x1,y1), P2(x2,y2) {}

	static Rect FromWidth(float x, float y, float w, float h) { return Rect(x,y,x+w,y+h); }

	float Width() const { return P2.X - P1.X; }
	float Height() const { return P2.Y - P1.Y; }

	bool Overlaps(const Rect& obj2rect) const { return (P2.X > obj2rect.P1.X)&&(P1.X < obj2rect.P2.X)&&(P2.Y > obj2rect.P1.Y)&&(P1.Y < obj2rect.P2.Y); }
	bool Contains(const Vec& p) const { return p.X>=P1.X && p.X<=P2.X && p.Y>=P1.X && p.Y<=P2.X; }
	Rect GetIntersection(const Rect& other) const
	{
		if(Overlaps(other))
		{
			return Rect(max(P1.X, other.P1.X), max(P1.Y, other.P1.Y),
				min(P2.X, other.P2.X), min(P2.Y, other.P2.Y));
		}

		return Rect(0,0,0,0);
	}
};

///************************************************************************/
/// Circle
///************************************************************************/

struct Circle
{
	Vec Center;
	float Radius;

	Circle(const Vec& c, float r) : Center(c), Radius(r) {}

	bool IsInside(const Vec& c) const { return (Center-c).LengthSquared() < Radius*Radius; }
	bool Overlaps(const Circle& c) const { return (Center-c.Center).LengthSquared() < (Radius+c.Radius)*(Radius+c.Radius); }
};

///************************************************************************/
/// Segment
///************************************************************************/

struct Segment
{
	Vec Start;
	Vec End;

	Segment(const Vec& st, const Vec& end) : Start(st), End(end) {}

	float Length() const { return (Start-End).Length(); }
	float LengthSquared() const { return (Start-End).LengthSquared(); }

	float Distance(const Vec& p) const;
	float AsLineDistance(const Vec& p) const;

	Vec Bounce(const Vec& p) const;
};

///************************************************************************/
/// Color
///************************************************************************/

struct Color
{
	float R;
	float G;
	float B;
	float A;

	Color() : R(1.0f), G(1.0f), B(1.0f), A(1.0f) {}
	Color(float r, float g, float b, float a = 1.0f) : R(r), G(g), B(b), A(a) {}

	Color operator*(const float f) const { return Color(R*f,G*f,B*f,A); }

	Color Lighten(const float coef) const {
		float rgb_max = Max3(R, G, B);
		float rgb_min = Min3(R, G, B);
		Color lighter = *this * (1.0f/rgb_max);
		//float dif = (rgb_max-rgb_min)/rgb_max;
		float dif = rgb_max;
		return (Color(lighter.R + dif * coef, lighter.G + dif * coef, lighter.B + dif * coef, A) * rgb_max).Clamped();
	}

	Color Clamped() const {
		return Color(min(1.0f, max(0.0f, R) ), min(1.0f, max(0.0f, G) ), min(1.0f, max(0.0f, B) ), min(1.0f, max(0.0f, A) ));
	}

	static Color White(float a = 1.0f) { return Color(1,1,1,a); }
	static Color Black(float a = 1.0f) { return Color(0,0,0,a); }
	static Color Red(float a = 1.0f) { return Color(1,0,0,a); }
	static Color Green(float a = 1.0f) { return Color(0,1,0,a); }
	static Color Blue(float a = 1.0f) { return Color(0,0,1,a); }
	static Color Cyan(float a = 1.0f) { return Color(0,1,1,a); }
	static Color Magenta(float a = 1.0f) { return Color(1,0,1,a); }
	static Color Yellow(float a = 1.0f) { return Color(1,1,0,a); }
	static Color Gray(float a = 1.0f) { return Color(0.5f,0.5f,0.5f,a); }
	static Color Transparent() { return Color(0,0,0,0); }
	static Color None() { return Color(0,0,0,-100.0f); }

	static inline float ByteToFloat(uint byte) { return (byte&0xFF)/255.0f; }
	static Color FromRGBInt(uint rgb) { return Color(ByteToFloat(rgb>>16),ByteToFloat(rgb>>8),ByteToFloat(rgb)); }
	static Color FromRGB(ubyte r, ubyte g, ubyte b) { return Color(r/255.0f,g/255.0f,b/255.0f); }

	bool IsNone() const { return A<-1.0f; }
};

struct Matrix
{
	Vec3 Row[3];

	/// elms 0-2 is row[0], elms 3-5 is row[1], elms 6-8 is row[2];
	float* asFloatArr()
	{
		///Vec3 size is equal to size of 3 floats. No virtual functions, no deriving
		///using reinterpret_cast explicit ensures no futile tries of other casts
		return reinterpret_cast<float*>(Row);
	}
	Matrix() { }
	Matrix(const Matrix& src) { *this = src; }
	Matrix(const Vec3& row1, const Vec3& row2, const Vec3& row3) { Row[0] = row1; Row[1] = row2; Row[2] = row3; }

	static const Matrix& Identity()
	{
		static Matrix identity = Matrix(Vec3(1,0,0), Vec3(0,1,0), Vec3(0,0,1));
		return identity;
	}

	static Matrix Translation(const Vec& Vec)
	{
		return Translation(Vec.X, Vec.Y);
	}

	static Matrix Translation(float x, float y)
	{
		Matrix translated = Identity();
		translated.Row[2].X = x; 
		translated.Row[2].Y = y; 
		return translated;
	}

	static Matrix Scale(float x, float y)
	{
		Matrix scale;
		scale.Row[0].V[0] = x;
		scale.Row[1].V[1] = y;
		scale.Row[2].V[2] = 1;
		return scale;
	}

	static Matrix Scale(const Vec& pivot, const Vec& scale)
	{
		return Scale(pivot, scale.X, scale.Y);
	}

	static Matrix Scale(const Vec& pivot, float scaleX, float scaleY)
	{
		return Translation(-pivot)*Scale(scaleX, scaleY)*Translation(pivot);
	}

	static Matrix Rotation(float radians)
	{
		Matrix rotation;
		float c = cos(radians);
		float s = sin(radians);
		rotation.Row[0].V[0] = c; rotation.Row[0].V[1] = s;
		rotation.Row[1].V[0] = -s; rotation.Row[1].V[1] = c;
		rotation.Row[2].V[2] = 1;
		return rotation;
	}

	static Matrix Rotation(const Vec& pivot, float angleRadians)
	{
		return Translation(-pivot)*Rotation(angleRadians)*Translation(pivot);
	}

	static Matrix RotationDegrees(float degrees)
	{
		return Rotation(3.1415926535f*2.0f*degrees/360.0f);
	}

	Matrix operator*(const Matrix& right) const
	{
		Matrix res;
		for(int i = 0; i<3; i++) /// res row
		{
			for(int j = 0; j<3; j++) /// res column
			{
				for(int k = 0; k<3; k++)
				{
					res.Row[i].V[j] += this->Row[i].V[k] * right.Row[k].V[j];
				}
			}
		}
		return res;
	}

	Matrix& operator=(const Matrix& src)
	{
		for(int i=0; i<3; i++)
		{
			Row[i] = src.Row[i];
		}
		return *this;
	}

	Matrix operator/(float k) const
	{
		k = 1.0f/k;
		Matrix res(*this);
		for(int r = 0; r<3; r++)
		{
			for(int c =0; c<3; c++)
			{
				res.Row[r].V[c] *= k;
			}
		}
		return res;
	}

	float GetRotation() const
	{
		return atan2(Row[0].V[1], Row[0].V[0]);
	}

	Vec GetTranslation() const
	{
		return Vec(Row[2].V[0], Row[2].V[1]);
	}

	Vec GetScale() const
	{
		return Vec(sqrtf(Row[0].X*Row[0].X+Row[0].Y*Row[0].Y), sqrtf(Row[1].X*Row[1].X+Row[1].Y*Row[1].Y));
	}

	Vec TransformVec(const Vec& src) const
	{
		Vec3 src3(src.X, src.Y, 1);
		Vec res;
		for(int elm = 0; elm < 2; elm ++)
		{
			for(int matrixRow = 0; matrixRow < 3; matrixRow ++ )
			{
				res.A[elm] += src3.V[matrixRow] * Row[matrixRow].V[elm];
			}
		}
		return res;
	}

	Vec TransformToLocal(const Vec& src) const
	{
		//printf("src: X = %f Y = %f\n", src.X, src.Y);
		return Inverse().TransformVec(src);
	}

	///Computes inverse matrix. Returns this and called from a degenerated matrix
	/// SLOW
	Matrix Inverse() const
	{
		float det = Det();
		if(fabs(det)>0.00000001)
		{
			det = 1.0f/det;

			Matrix inverted;
			///Compute adjugate matrix
			for(int r = 0; r<3; r++)
			{
				for(int c =0; c<3; c++)
				{
					int mul = (r+c)%2==0?1:-1;
					//inverted.Row[r].V[c] = float(mul*transposed.MinorDet(First(c), Last(r), Last(c), First(r)))*det;
					inverted.Row[r].V[c] = float(mul*MinorDetTransp(First(c), Last(r), Last(c), First(r)))*det;
				}
			}
			return inverted;
		}
		return *this;
	}

	///Get the first index of row or column for the provided excluded one
	inline int First(int excluded) const
	{
		return excluded==0?1:0;
	}
	///Get the last index of row or column for the provided excluded one
	inline int Last(int excluded) const
	{
		return excluded==2?1:2;
	}
	///Computes transposed matrix of this.
	Matrix Transpose() const
	{
		Matrix res;
		for(int r = 0; r<3; r++)
		{
			for(int c =0; c<3; c++)
			{
				res.Row[r].V[c] = Row[c].V[r];
			}
		}
		return res;
	}
	///Computes determinant of this matrix
	float Det() const
	{
		return (
			Row[0].V[0]*Row[1].V[1]*Row[2].V[2]
		+ Row[0].V[1]*Row[1].V[2]*Row[2].V[0]
		+	Row[1].V[0]*Row[2].V[1]*Row[0].V[2]
		- Row[0].V[2]*Row[1].V[1]*Row[2].V[0]
		- Row[1].V[0]*Row[0].V[1]*Row[2].V[2]
		- Row[0].V[0]*Row[2].V[1]*Row[1].V[2]);
	}

	///Gets the determinant of a minor matrix of this one
	inline float MinorDet(int left, int down, int right, int top) const
	{
		return Row[top].V[left]*Row[down].V[right] - Row[top].V[right]*Row[down].V[left];
	}

	inline float MinorDetTransp(int left, int down, int right, int top) const
	{
		return Row[left].V[top]*Row[right].V[down] - Row[right].V[top]*Row[left].V[down];
	}
};

///************************************************************************/
/// Functions
///************************************************************************/

inline bool VecInCircle(Vec center, float radius, Vec pos)
{
	return (pos-center).LengthSquared() <= radius*radius;
}

#endif // Geo_h__
